{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "9ed9eb03-31f0-42ff-8213-324dd47a6ec0",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import scipy as sp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "8f48fc07-f3ef-474f-a171-62ad9c39f8e5",
   "metadata": {},
   "outputs": [],
   "source": [
    "def rodrigez(fi,p): \n",
    "    e=np.eye(3)\n",
    "    px=np.array([[0,-p[2],p[1]],\n",
    "                 [p[2],0,-p[0]],\n",
    "                 [-p[1],p[0],0]])\n",
    "    ppT=np.array([[p[0]**2,p[0]*p[1],p[0]*p[2]]\n",
    "                 ,[p[1]*p[0],p[1]**2,p[1]*p[2]]\n",
    "                 ,[p[2]*p[0],p[2]*p[1],p[2]**2]])\n",
    "    return (1-np.cos(fi))*ppT+np.cos(fi)*e+np.sin(fi)*px"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "4fc62797-db61-4795-84ff-69624443375a",
   "metadata": {},
   "outputs": [],
   "source": [
    "def provera(A): # Provera za rotacije, tj. matrice kretanja\n",
    "    provera=A@A.T\n",
    "    provera=np.where(np.isclose(provera,0),0,provera)\n",
    "    provera=np.where(np.isclose(provera,1),1,provera)\n",
    "    if (A.shape[0]!=A.shape[1]) or (not np.isclose(np.linalg.det(A),1)) or not np.all(provera==np.identity(A.shape[0])):\n",
    "        return \"Nije matrica kretanja!\"\n",
    "    else:\n",
    "        return \"Sve ok!\""
   ]
  },
  {
   "cell_type": "markdown",
   "id": "24ea6026-1ade-48e6-ab78-bbc1926440c5",
   "metadata": {},
   "source": [
    "--------------------Odavde pocinje cetvrti domaci--------------------"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "e810afe1-8c57-4338-9069-26a5a8374912",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import scipy as sp\n",
    "import math\n",
    "\n",
    "class Quaternion:\n",
    "    def __init__(self,params,w):\n",
    "        self.i=params[0]\n",
    "        self.j=params[1]\n",
    "        self.k=params[2]\n",
    "        self.params=params\n",
    "        self.w=w\n",
    "        \n",
    "    def __truediv__(self,o:Quaternion)->Quaternion:\n",
    "        if isinstance(o,Quaternion):\n",
    "            donji=o.i**2+o.j**2+o.k**2+o.w**2\n",
    "            return Quaternion([(o.w*self.i-o.i*self.w-o.j*self.k+o.k*self.j)/donji,\n",
    "                              (o.w*self.j-o.i*self.k-o.j*self.w+o.k*self.i)/donji,\n",
    "                              (o.w*self.k-o.i*self.j-o.j*self.i+o.k*self.w)/donji],\n",
    "                              (o.w*self.w-o.i*self.i-o.j*self.j+o.k*self.k)/donji)\n",
    "        else:\n",
    "            a=[]\n",
    "            for x in self.params:\n",
    "                a.append(x/o)\n",
    "            return Quaternion(a,self.w/o)\n",
    "            \n",
    "    def __add__(self,o:Quaternion)->Quaternion:\n",
    "        if isinstance(o,Quaternion):\n",
    "            return Quaternion([self.i+o.i,self.j+o.j,self.k+o.k],self.w+o.w)\n",
    "        else:\n",
    "            return Quaternion(self.params,self.w+o)\n",
    "            \n",
    "    def __radd__(self,o):\n",
    "        if not isinstance(o,Quaternion) and not isinstance(o,str):\n",
    "            return self.__add__(o)\n",
    "\n",
    "    def __sub__(self,o:Quaternion)->Quaternion:\n",
    "        if isinstance(o,Quaternion):\n",
    "            return Quaternion([self.i-o.i,self.j-o.j,self.k-o.k],self.w-o.w)       \n",
    "        else:\n",
    "            return Quaternion(self.params,self.w-o)\n",
    "            \n",
    "    def __mul__(self,o:Quaternion)->Quaternion:\n",
    "        if isinstance(o,Quaternion): \n",
    "            return Quaternion([self.w*o.i+self.i*o.w+self.j*o.k-self.k*o.j,\n",
    "                               self.w*o.j+self.j*o.w+self.k*o.i-self.i*o.k,\n",
    "                               self.w*o.k+self.k*o.w+self.i*o.j-self.j*o.i],\n",
    "                               self.w*o.w-self.i*o.i-self.j*o.j-self.k*o.k)\n",
    "        else:\n",
    "            a=[]\n",
    "            for x in self.params:\n",
    "                a.append(x*o)\n",
    "            return Quaternion(a,self.w*o)\n",
    "\n",
    "    def __rmul__(self,o):\n",
    "        if not isinstance(o,Quaternion) and not isinstance(o,str):\n",
    "            return self.__mul__(o)\n",
    "            \n",
    "    def __imul__(self,o:Quaternion)->Quaternion:\n",
    "            return self.__mul__(o)\n",
    "    def __idiv__(self,o:Quaternion)->Quaternion:\n",
    "            return self.__truediv__(o)\n",
    "        \n",
    "    def konjugat(self)->Quaternion:\n",
    "        a=[]\n",
    "        for x in self.params:\n",
    "            a.append(-x)\n",
    "        return Quaternion(a,self.w)\n",
    "        \n",
    "    def norm(self)->float:\n",
    "        return np.sqrt(self.i**2+self.j**2+self.k**2+self.w**2)\n",
    "        \n",
    "    def dot(self,o:Quaternion)->float:\n",
    "        return self.i*o.i+self.j*o.j+self.k*o.k+self.w*o.w\n",
    "        \n",
    "    def inverse(self)->Quaternion:\n",
    "        q=(self.konjugat()/(self.norm()**2)) \n",
    "        return Quaternion(q.params,q.w)\n",
    "        \n",
    "    def __str__(self)->str:\n",
    "        return '['+str(self.i)+'i'+','+str(self.j)+'j'+','+str(self.k)+'k'+','+str(self.w)+']' \n",
    "    def __repr__(self)->str:\n",
    "        return '['+str(self.i)+'i'+','+str(self.j)+'j'+','+str(self.k)+'k'+','+str(self.w)+']'\n",
    "\n",
    "    def __neg__(self)->Quaternion:\n",
    "        fi,p=Q2AngleAxis(self)\n",
    "        return AngleAxis2Q(-fi,-p)\n",
    "\n",
    "    def __eq__(self,o:Quaternion)->bool:\n",
    "        return (self.i==o.i and self.j==o.j and self.k==o.k and self.w==o.w and self.params==o.params)\n",
    "\n",
    "    def to_rotation_matrix(self)->np.array: # Treba prvo normirati vektor pre pozivanja funkcije\n",
    "        return np.array([[1-2*(self.j**2+self.k**2),2*(self.i*self.j+self.w*self.k),2*(self.i*self.k-self.w*self.j)],\n",
    "                         [2*(self.i*self.j-self.w*self.k),1-2*(self.i**2+self.k**2),2*(self.j*self.k+self.w*self.i)],\n",
    "                         [2*(self.i*self.k+self.w*self.j),2*(self.j*self.k-self.w*self.i),1-2*(self.i**2+self.j**2)]])\n",
    "\n",
    "#dodato zbog predstavljanja SLerpa pomocu stepena (videcemo da li radi)\n",
    "    def vector_part(self)->Quaternion:\n",
    "        return (self-self.konjugat())/2\n",
    "    def sign(self)->Quaternion:\n",
    "        return self/self.norm()\n",
    "    def arg(self)->float:\n",
    "        return np.arccos(w/self.norm())\n",
    "    def exp(self):\n",
    "        return (self.vector_part().sign()*np.sin(self.vector_part().norm())+np.cos(self.vector_part().norm()))*math.exp(self.w)\n",
    "    def ln(self):\n",
    "        return self.sign()*np.sin(self.vector_part().norm())+math.log(self.norm())\n",
    "    def __pow__(self,o:int):\n",
    "        return (self.ln()*o).exp()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "id": "94f9cc1d-73e3-420d-bb9e-1cde7eb8c5a6",
   "metadata": {},
   "outputs": [],
   "source": [
    "#verzija za tester\n",
    "class Quaternion:\n",
    "    def __init__(self,params):\n",
    "        self.i=params[0]\n",
    "        self.j=params[1]\n",
    "        self.k=params[2]\n",
    "        self.w=params[3]\n",
    "        self.params=params\n",
    "        \n",
    "    def __truediv__(self,o:Quaternion)->Quaternion:\n",
    "        if isinstance(o,Quaternion):\n",
    "            donji=o.i**2+o.j**2+o.k**2+o.w**2\n",
    "            return Quaternion([(o.w*self.i-o.i*self.w-o.j*self.k+o.k*self.j)/donji,\n",
    "                              (o.w*self.j-o.i*self.k-o.j*self.w+o.k*self.i)/donji,\n",
    "                              (o.w*self.k-o.i*self.j-o.j*self.i+o.k*self.w)/donji],\n",
    "                              (o.w*self.w-o.i*self.i-o.j*self.j+o.k*self.k)/donji)\n",
    "        else:\n",
    "            a=[]\n",
    "            for x in self.params:\n",
    "                a.append(x/o)\n",
    "            return Quaternion(a)\n",
    "            \n",
    "    def __add__(self,o:Quaternion)->Quaternion:\n",
    "        if isinstance(o,Quaternion):\n",
    "            return Quaternion([self.i+o.i,self.j+o.j,self.k+o.k,self.w+o.w])\n",
    "        else:\n",
    "            a=[self.i,self.j,self.k,self.w+o]\n",
    "            return Quaternion(a)\n",
    "            \n",
    "    def __radd__(self,o):\n",
    "        if not isinstance(o,Quaternion) and not isinstance(o,str):\n",
    "            return self.__add__(o)\n",
    "\n",
    "    def __sub__(self,o:Quaternion)->Quaternion:\n",
    "        if isinstance(o,Quaternion):\n",
    "            return Quaternion([self.i-o.i,self.j-o.j,self.k-o.k,self.w-o.w])       \n",
    "        else:\n",
    "            a=[self.i,self.j,self.k,self.w-o]\n",
    "            return Quaternion(a)\n",
    "            \n",
    "    def __mul__(self,o:Quaternion)->Quaternion:\n",
    "        if isinstance(o,Quaternion): \n",
    "            return Quaternion([self.w*o.i+self.i*o.w+self.j*o.k-self.k*o.j,\n",
    "                               self.w*o.j+self.j*o.w+self.k*o.i-self.i*o.k,\n",
    "                               self.w*o.k+self.k*o.w+self.i*o.j-self.j*o.i],\n",
    "                               self.w*o.w-self.i*o.i-self.j*o.j-self.k*o.k)\n",
    "        else:\n",
    "            a=[]\n",
    "            for x in self.params:\n",
    "                a.append(x*o)\n",
    "            return Quaternion(a)\n",
    "\n",
    "    def __rmul__(self,o):\n",
    "        if not isinstance(o,Quaternion) and not isinstance(o,str):\n",
    "            return self.__mul__(o)\n",
    "            \n",
    "    def __imul__(self,o:Quaternion)->Quaternion:\n",
    "            return self.__mul__(o)\n",
    "    def __idiv__(self,o:Quaternion)->Quaternion:\n",
    "            return self.__truediv__(o)\n",
    "        \n",
    "    def konjugat(self)->Quaternion:\n",
    "        a=[-self.i,-self.j,-self.k,self.w]\n",
    "        return Quaternion(a)\n",
    "        \n",
    "    def norm(self)->float:\n",
    "        return np.sqrt(self.i**2+self.j**2+self.k**2+self.w**2)\n",
    "    def inverse(self)->Quaternion:\n",
    "        q=(self.konjugat()/(self.norm()**2)) \n",
    "        return Quaternion(q.params)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "279c13c8-04e2-41e2-96c2-3d01aeb1deaa",
   "metadata": {},
   "outputs": [],
   "source": [
    "def AngleAxis2Q(fi,p):\n",
    "    w=np.cos(fi/2)\n",
    "    p/=np.linalg.norm(p)\n",
    "    params=np.sin(fi/2)*p\n",
    "    return Quaternion(params,w)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "id": "5121c180-fa8e-4dee-ba33-cd00b85ce474",
   "metadata": {},
   "outputs": [],
   "source": [
    "# def AxisAngle2Q(pphi):  # Ovo je verzija za tester\n",
    "#     p=[pphi[0],pphi[1],pphi[2]]\n",
    "#     w=np.cos(pphi[3]/2)\n",
    "#     p/=np.linalg.norm(p)\n",
    "#     params=np.sin(pphi[3]/2)*p\n",
    "#     return np.array([params[0],params[1],params[2],w])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 72,
   "id": "a1cc9260-8884-4392-8b06-7a8fb8ae30ca",
   "metadata": {},
   "outputs": [],
   "source": [
    "def Q2AngleAxis(q:Quaternion):\n",
    "    q/=q.norm()\n",
    "    if q.w<0:\n",
    "        q=q*(-1)\n",
    "    fi=2*np.arccos(q.w)\n",
    "    if abs(q.w) == 1:\n",
    "        p=np.array[1,0,0]\n",
    "    else:\n",
    "        p=q.params\n",
    "        p/=np.linalg.norm(p)\n",
    "    return fi,p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 97,
   "id": "b6c82181-686c-4021-822a-2d41683a2913",
   "metadata": {},
   "outputs": [],
   "source": [
    "def Q2AxisAngle(q): #verzija za tester\n",
    "    q=Quaternion(q)\n",
    "    q/=q.norm()\n",
    "    if q.w<0:\n",
    "        q=q*(-1)\n",
    "    fi=2*np.arccos(q.w)\n",
    "    if abs(q.w) == 1:\n",
    "        return np.array([1,0,0,0])\n",
    "    else:\n",
    "        p=q.params[:-1]\n",
    "        p/=np.linalg.norm(p)\n",
    "    pphi=np.array([p[0],p[1],p[2],fi])\n",
    " \n",
    "    pphi = np.where(np.isclose(pphi, 0) , 0 , pphi)  # izbegavanje -0. u rezultatu\n",
    "    return pphi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 96,
   "id": "44e6782e-316e-44c6-953e-135f6c687593",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[1 0 0 0]\n"
     ]
    }
   ],
   "source": [
    "print(Q2AxisAngle(np.array([0, 0,0,-3])))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "eaee072a-2773-433c-9633-7b5158d7bd11",
   "metadata": {},
   "outputs": [],
   "source": [
    "def Konjugacija(p:Quaternion,q:Quaternion)->Quaternion:\n",
    "    #Cq(p)=qpq^-1\n",
    "    return q*p*q.inverse()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "136fb4f4-02ff-48a0-b9aa-b181ea4ae738",
   "metadata": {},
   "outputs": [],
   "source": [
    "def SLerp(q1:Quaternion,q2:Quaternion,tm:int):\n",
    "    cos0=q1.dot(q2)\n",
    "    #print(cos0)\n",
    "    if cos0 < 0:\n",
    "        q1=q1*(-1)\n",
    "        cos0=-cos0\n",
    "    if cos0 > 0.95:\n",
    "        q1=q1*(-1) #ili lerp interpolaciju\n",
    "    fi=np.arccos(cos0)\n",
    "    return [q1*(np.sin(fi*(1-i/tm))/np.sin(fi))\n",
    "            +q2*(np.sin(fi*(i/tm))/np.sin(fi)) for i in range(tm)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2dc02ecc-88a9-49ea-bea2-39808de781fe",
   "metadata": {},
   "outputs": [],
   "source": [
    "q1=Quaternion([3,0,-5],1)\n",
    "q2=Quaternion([1,1,0],7)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fdb009a7-26d2-4976-831b-230d7c60363b",
   "metadata": {},
   "outputs": [],
   "source": [
    "q7=Quaternion([1,0,2],2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2a14364d-84c6-4b7b-b12b-998f4bd8482e",
   "metadata": {},
   "outputs": [],
   "source": [
    "(q1/q1.norm()).to_rotation_matrix()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d396f63b-21c6-40ed-94e6-47531dfb65ad",
   "metadata": {},
   "outputs": [],
   "source": [
    "provera((q1/q1.norm()).to_rotation_matrix())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8b6a9f79-23d4-4325-a5cf-a572a90aa461",
   "metadata": {},
   "outputs": [],
   "source": [
    "q7.norm()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "df742c27-351d-4e7a-b676-56112dc943ca",
   "metadata": {},
   "outputs": [],
   "source": [
    "q8=Quaternion([0,np.sqrt(3)/2,0],1/2)\n",
    "Konjugacija(Quaternion([0,0,1],0),q8)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "6ddb1a87-946c-4a43-874c-b47c676c3714",
   "metadata": {},
   "outputs": [],
   "source": [
    "q10=Quaternion([1,3,1],-7)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "0ad5e66a-c7e1-4922-934d-fa6426c66764",
   "metadata": {},
   "outputs": [],
   "source": [
    "q11=Quaternion([-5,-5,7],1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "5aa5a66b-b4d3-440a-8283-d1f6731c5a29",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-4i,-2j,8k,-6]"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q10+q11"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "b73c8683-d119-436e-b424-446517cf639d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[62i,26j,-38k,6]"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q10*q11"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "0b9bd725-06e5-4d26-8fe8-5db6a3073b0d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[-0.16666666666666666i,-0.8333333333333334j,-0.5k,0.16666666666666666]"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q12=Quaternion([1,5,3],1)\n",
    "6*q12.inverse()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "df5b3bfb-394b-436b-b79b-2f2f1c57200d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-0., -1., -0.])"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q13=Quaternion([0,np.sqrt(3)/2,0],-(1/2))\n",
    "ugao,osa=Q2AngleAxis(q13)\n",
    "ugao=np.degrees(ugao)\n",
    "ugao\n",
    "osa"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "88fc8209-87e1-4679-98b7-c234125641b1",
   "metadata": {},
   "source": [
    "--------------------------Animacija--------------------------------"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ff261057-7dd7-4949-8067-cc3f42aa8eeb",
   "metadata": {},
   "outputs": [],
   "source": [
    "#%pip install manim"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "375dd88d-0261-4f16-8609-f10f77e4975c",
   "metadata": {},
   "outputs": [],
   "source": [
    "import manim as mm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7aada3f9-79c0-408f-bbe5-45a0c9f2386c",
   "metadata": {
    "jupyter": {
     "source_hidden": true
    }
   },
   "outputs": [],
   "source": [
    "mm.interpolate()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 98,
   "id": "b57e17ce-3edf-4771-97b7-71dd95db61eb",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from numpy import linalg\n",
    "import math\n",
    "np.set_printoptions(precision=5, suppress=True) \n",
    "\n",
    "# pomocne funkcije, ako treba\n",
    "\n",
    "def AxisAngle2Q(pphi):\n",
    "    p=[pphi[0],pphi[1],pphi[2]]\n",
    "    w=np.cos(pphi[3]/2)\n",
    "    p/=np.linalg.norm(p)\n",
    "    params=np.sin(pphi[3]/2)*p\n",
    "    q=np.array([params[0],params[1],params[2],w])\n",
    "    q = np.where(np.isclose(q, 0) , 0 , q)  # izbegavanje -0. u rezultatu\n",
    "    return q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8c22491d-4ccd-44f3-8898-c8bc4cdee221",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from numpy import linalg\n",
    "import math\n",
    "np.set_printoptions(precision=5, suppress=True) \n",
    "\n",
    "#verzija za tester\n",
    "class Quaternion:\n",
    "    def __init__(self,params):\n",
    "        self.i=params[0]\n",
    "        self.j=params[1]\n",
    "        self.k=params[2]\n",
    "        self.w=params[3]\n",
    "        self.params=params\n",
    "        \n",
    "    def __truediv__(self,o):\n",
    "            a=[]\n",
    "            for x in self.params:\n",
    "                a.append(x/o)\n",
    "            return Quaternion(a)\n",
    "            \n",
    "    def __add__(self,o):\n",
    "            a=[self.i,self.j,self.k,self.w+o]\n",
    "            return Quaternion(a)\n",
    "            \n",
    "    def __sub__(self,o):\n",
    "            a=[self.i,self.j,self.k,self.w-o]\n",
    "            return Quaternion(a)\n",
    "            \n",
    "    def __mul__(self,o):\n",
    "            a=[]\n",
    "            for x in self.params:\n",
    "                a.append(x*o)\n",
    "            return Quaternion(a)\n",
    "\n",
    "            \n",
    "    def __imul__(self,o):\n",
    "            return self.__mul__(o)\n",
    "    def __idiv__(self,o):\n",
    "            return self.__truediv__(o)\n",
    "        \n",
    "    def konjugat(self):\n",
    "        a=[-self.i,-self.j,-self.k,self.w]\n",
    "        return Quaternion(a)\n",
    "        \n",
    "    def norm(self)->float:\n",
    "        return np.sqrt(self.i**2+self.j**2+self.k**2+self.w**2)\n",
    "\n",
    "\n",
    "def Q2AxisAngle(q): #verzija za tester\n",
    "    q=Quaternion(q)\n",
    "    q/=q.norm()\n",
    "    if q.w<0:\n",
    "        q=q*(-1)\n",
    "    fi=2*np.arccos(q.w)\n",
    "    if abs(q.w) == 1:\n",
    "        return np.array([1,0,0,0])\n",
    "    else:\n",
    "        p=q.params[:-1]\n",
    "        p/=np.linalg.norm(p)\n",
    "    pphi=np.array([p[0],p[1],p[2],fi])\n",
    " \n",
    "    pphi = np.where(np.isclose(pphi, 0) , 0 , pphi)  # izbegavanje -0. u rezultatu\n",
    "    return pphi\n",
    " \n",
    " "
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
